﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using System.Collections;
using ProjetoKinect.enums;
using System.Windows.Controls;
namespace ProjetoKinect.Model
{
    enum HandLocation { LU, LM, LD, MU, MM, MD, RU, RM, RD, DEF };
    //LU leftUp = left side of body and up shoulder
    //LM leftMiddle = left side of body and between shoulder and waist 
    //LD leftDown = left side of body and under waist 
    //MU middleUp = Between left and right shoulders and up shoulder
    //MM middleMiddle = Between left and right shoulders and between shoulder and waist 
    //MD middleDown = Between left and right shoulders and under waist 
    //RU rightUp = Right side of body and up shoulder
    //RM rightMiddle = Right side of body and between shoulder and waist 
    //RD rightDown = Right side of body and under waist
    //DEF = default 
    class BodyControl
    {
        private List<MyJoint> myJointsList = new List<MyJoint>();
        private Pose pose = Pose.NONE;
        private DateTime poseDate= DateTime.Now;
      
        public HandLocation getLeftHandLocation()
        {
            MyJoint handJoint = getMyJointByJointType(JointType.HandLeft);
            float handX = handJoint.getLastX();
            float handY = handJoint.getLastY();

            MyJoint leftShoulderJoint = getMyJointByJointType(JointType.ShoulderLeft);
            MyJoint leftHipJoint = getMyJointByJointType(JointType.HipLeft);
            float leftShoulderX = leftShoulderJoint.getLastX();
            float leftShoulderY = leftShoulderJoint.getLastY();
            float leftHipX = leftHipJoint.getLastX();
            float leftHipY = leftHipJoint.getLastY();

            MyJoint rightShoulderJoint = getMyJointByJointType(JointType.ShoulderRight);
            MyJoint rightHipJoint = getMyJointByJointType(JointType.HipRight);
            float rightShoulderX = rightShoulderJoint.getLastX();
            float rightShoulderY = rightShoulderJoint.getLastY();
            float rightHipX = rightHipJoint.getLastX();
            float rightHipY = rightHipJoint.getLastY();

            if (handX < leftShoulderX && handY >= leftShoulderY)
            {
                return HandLocation.LU;
            }
            else if (handX < leftShoulderX && handY < leftShoulderY && handY > leftHipY && handX < leftHipX)
            {
                return HandLocation.LM;
            }
            else if (handX < leftHipX && handY <= leftHipY)
            {
                return HandLocation.LD;
            }
            else if (handX >= leftShoulderX && handX <= rightShoulderX)
            {
                if (handY >= leftShoulderY)
                {
                    return HandLocation.MU;
                }
                else if (handY <= leftHipY)
                {
                    return HandLocation.MD;
                }
                else if (handY <= leftShoulderY && handY >= leftHipY)
                {
                    return HandLocation.MM;
                }
                else
                {
                    return HandLocation.DEF;
                }
            }
            else
            {
                return HandLocation.DEF;
            }
        }

        public HandLocation getRightHandLocation()
        {
            MyJoint handJoint = getMyJointByJointType(JointType.HandRight);
            float rightHandX = handJoint.getLastX();
            float rightHandY = handJoint.getLastY();

            MyJoint rightShoulderJoint = getMyJointByJointType(JointType.ShoulderRight);
            MyJoint rightHipJoint = getMyJointByJointType(JointType.HipRight);
            float rightShoulderX = rightShoulderJoint.getLastX();
            float rightShoulderY = rightShoulderJoint.getLastY();
            float rightHipX = rightHipJoint.getLastX();
            float rightHipY = rightHipJoint.getLastY();

            MyJoint leftShoulderJoint = getMyJointByJointType(JointType.ShoulderLeft);
            MyJoint leftHipJoint = getMyJointByJointType(JointType.HipLeft);
            float leftShoulderX = leftShoulderJoint.getLastX();
            float leftShoulderY = leftShoulderJoint.getLastY();
            float leftHipX = leftHipJoint.getLastX();
            float leftHipY = leftHipJoint.getLastY();

            if (rightHandX >= rightShoulderX && rightHandY >= rightShoulderY)
            {
                return HandLocation.RU;
            }
            else if (rightHandX > rightShoulderX && rightHandY < rightShoulderY && rightHandY > rightHipY && rightHandX > rightHipX)
            {
                return HandLocation.RM;
            }
            else if (rightHandX >= rightHipX && rightHandY <= rightHipY)
            {
                return HandLocation.RD;
            }
            else if (rightHandX <= rightShoulderX && rightHandX >= leftShoulderX) {
                if (rightHandY >= rightShoulderY) {
                    return HandLocation.MU;
                }
                else if (rightHandY < rightShoulderY && rightHandY > rightHipY) {
                    return HandLocation.MM;
                }
                else if (rightHandY <= rightHipY) {
                    return HandLocation.MD;
                }
                else
                {
                    return HandLocation.DEF;
                }
            }
            else
            {
                return HandLocation.DEF;
            }
        }
       
        internal void addJointPositions(Joint joint)
        {
            MyJoint myJoint = getMyJointByJointType(joint.JointType);
            if (myJoint != null) {
                myJoint.addPosition(joint.Position);
            }else{
                myJointsList.Add(new MyJoint(joint));
            }
        }

        public MyJoint getMyJointByJointType(JointType jointType)
        {
            foreach (MyJoint myJoint in myJointsList) {
                if (myJoint.getJointType().Equals(jointType)) {
                    return myJoint;
                }
            }
            return null;
        }
     
        internal void setSkeletonJoints(Skeleton skeleton)
        {

                Joint rightHand = skeleton.Joints[JointType.HandRight];
                Joint rightShoulder = skeleton.Joints[JointType.ShoulderRight];
                Joint rightHip = skeleton.Joints[JointType.HipRight];
                Joint rightElbow = skeleton.Joints[JointType.ElbowRight];
                
                Joint lefthand = skeleton.Joints[JointType.HandLeft];
                Joint leftShoulder = skeleton.Joints[JointType.ShoulderLeft];
                Joint leftHip = skeleton.Joints[JointType.HipLeft];
                Joint leftElbow = skeleton.Joints[JointType.ElbowLeft];

                addJointPositions(rightHand);
                addJointPositions(rightShoulder);
                addJointPositions(rightHip);
                addJointPositions(rightElbow);

                addJointPositions(lefthand);
                addJointPositions(leftShoulder);
                addJointPositions(leftHip);
                addJointPositions(leftElbow);
            
            
        }

        public MovimentStatus getJointMovimentStatus(JointType jointType,Axis axis)
        {
            MyJoint joint = getMyJointByJointType(jointType);
            JointMoviment jointMoviment = joint.getJointMovimentByAxis(axis);
            //default 4
            return jointMoviment.getMovimentStatus();
        }
        public MovimentStatus getJointMovimentStatus(JointType jointType, Axis axis, int frames)
        {
            //quanto menor o framesCount, menor a sensibilidade a movimentação
            MyJoint joint = getMyJointByJointType(jointType);
            JointMoviment jointMoviment = joint.getJointMovimentByAxis(axis);
            return jointMoviment.getMovimentStatus(frames);
        }

        public bool isJoinDistantFromJointBase(JointType jointType, JointType JointTypeBase)
        {
            MyJoint joint = getMyJointByJointType(jointType);
            float jointPosition = joint.getJointMovimentByAxis(Axis.AXIS_Z).getLastElementPosition();

            MyJoint jointBase = getMyJointByJointType(JointTypeBase);
            float jointBasePosition = jointBase.getJointMovimentByAxis(Axis.AXIS_Z).getLastElementPosition();

            if (jointBasePosition - jointPosition >= 0.3)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        public bool isJoinZDistantFromJointBase(JointType jointType, JointType JointTypeBase, float distance,TextBlock textBlock)
        {
            MyJoint joint = getMyJointByJointType(jointType);
            float jointPosition = joint.getJointMovimentByAxis(Axis.AXIS_Z).getLastElementPosition();

            MyJoint jointBase = getMyJointByJointType(JointTypeBase);
            float jointBasePosition = jointBase.getJointMovimentByAxis(Axis.AXIS_Z).getLastElementPosition();
            if (textBlock != null)
            {
                textBlock.Text = jointType + " = " + jointPosition + " // " + JointTypeBase + " = " + jointBasePosition + "//d= " + distance;
            }
            if (jointBasePosition - jointPosition >= distance)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool areHandsOnHip() {
            MyJoint leftHand = getMyJointByJointType(JointType.HandLeft);
            MyJoint rightHand = getMyJointByJointType(JointType.HandRight);
            MyJoint leftHip = getMyJointByJointType(JointType.HipLeft);
            MyJoint rightHip = getMyJointByJointType(JointType.HipRight);

            float leftHandY = leftHand.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightHandY = rightHand.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float leftHipY = leftHip.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightHipY = rightHip.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            if (leftHandY == leftHipY && rightHandY == rightHipY)
            {
                return true;
            }
            else {
                return false;
            }
        }
        public bool isSurrenderPose(TextBlock textBlock)
        {
            MyJoint leftHand = getMyJointByJointType(JointType.HandLeft);
            MyJoint rightHand = getMyJointByJointType(JointType.HandRight);
            MyJoint leftShoulder = getMyJointByJointType(JointType.ShoulderLeft);
            MyJoint rightShoulder = getMyJointByJointType(JointType.ShoulderRight);
            MyJoint leftElbow = getMyJointByJointType(JointType.ElbowLeft);
            MyJoint rightElbow = getMyJointByJointType(JointType.ElbowRight);

            float leftHandX = leftHand.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float leftHandY = leftHand.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightHandX = rightHand.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float rightHandY = rightHand.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
           

            float leftShoulderY = leftShoulder.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float leftShoulderX = leftShoulder.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float leftShoulderXAbsolut = leftShoulder.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();

            float rightShoulderY = rightShoulder.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightShoulderX = rightShoulder.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float rightShoulderXAbsolut = rightShoulder.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();

            float leftElbowY = leftElbow.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float leftElbowX = leftElbow.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float leftElbowXAbsolut = leftElbow.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();

            float rightElbowY = rightElbow.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightElbowX = rightElbow.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float rightElbowXAbsolut = rightElbow.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();
            if (!isJoinZDistantFromJointBase(JointType.HandLeft, JointType.ShoulderLeft,0.2f,textBlock))
            {
                float xRightDiference = rightElbowXAbsolut - rightShoulderXAbsolut;
                float xLeftDiference = leftShoulderXAbsolut - leftElbowXAbsolut;
                if (textBlock != null)
                {
                   // textBlock.Text = "LSY = " + leftShoulderY + " //LEY = " + leftElbowY + " //LHY = " + leftHandY;
                    //textBlock.Text =     "LSX = " + leftShoulderX + " //LEX = " + leftElbowX + " //LHY = " + leftHandX;
                        //+ leftElbowY + " " + "RSY = " + rightShoulderY + "//REY = " + rightElbowY;
                    //                textBlock.Text = "xRightDiff = " + xRightDiference + "//xLeftDif = " + xLeftDiference;
                }
                if (areSimilar(leftShoulderY, leftElbowY) && (areSimilar(rightShoulderY,rightElbowY)))
                {
                    if (areSimilar(leftElbowX,leftHandX) && areSimilar(rightElbowX,rightHandX))
                    {
                        
                        if ((xRightDiference > 0.15 && xLeftDiference > 0.15) &&
                            (rightElbowY < rightHandY && leftElbowY < leftHandY))
                        {
                            setPose(Pose.SURRENDER);
                            return true;
                            
                        }
                    }
                }
            }
                return false;
            
        }

        public void setPose(Pose pose)
        {
            if (this.pose != pose) {
                this.pose = pose;
                poseDate = DateTime.Now;
            }
        }
        public String getPoseMessage() {
            String message = "";
            float secCount = getSecCount();
            
            if (this.pose == Pose.ZOOMBIE) {
                
                message = " Configuração " + (secCount);
            }
            else if (this.pose == Pose.SURRENDER)
            {
                message = "Sair :" + (secCount);
            }
            else
            {
                message = "Gesto não detectado";
            }
            return message;
        }

        public int getSecCount(){
            int result = DateTime.Now.Second - poseDate.Second;
            if (result < 0) {
                result = result * -1;
            }
            return result;
           // return DateTime.Now.Millisecond - poseDate.Millisecond;
        }

        internal bool isLeftHandInVerticalMiddle() {
            return isHandInVerticalMiddle(getLeftHandLocation());
        }

        internal bool isRightHandInVerticalMiddle()
        {
            return isHandInVerticalMiddle(getRightHandLocation());
        }

        internal bool isHandInVerticalMiddle(HandLocation handLocation)
        {
            if (handLocation == HandLocation.MD || handLocation == HandLocation.MM || handLocation == HandLocation.MU)
            {
                return true;
            }
            else {
                return false;
            }
        }

        internal bool isHandInHorizontalMiddle(HandLocation handLocation)
        {
            if (handLocation == HandLocation.LM || handLocation == HandLocation.MM || handLocation == HandLocation.RM)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
       
        internal bool isLeftHandInHorizontalMiddle()
        {
            return isHandInHorizontalMiddle(getLeftHandLocation());
        }

        internal bool isRightHandInHorizontalMiddle()
        {
            return isHandInHorizontalMiddle(getRightHandLocation());
        }

        internal bool isTwoHandsFrontPosition(TextBlock selectedFunction)
        {
            MyJoint leftHand = getMyJointByJointType(JointType.HandLeft);
            MyJoint rightHand = getMyJointByJointType(JointType.HandRight);
            MyJoint leftShoulder = getMyJointByJointType(JointType.ShoulderLeft);
            MyJoint rightShoulder = getMyJointByJointType(JointType.ShoulderRight);
            MyJoint leftElbow = getMyJointByJointType(JointType.ElbowLeft);
            MyJoint rightElbow = getMyJointByJointType(JointType.ElbowRight);

            float leftHandX = leftHand.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float rightHandX = rightHand.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();


            float leftShoulderY = leftShoulder.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float leftShoulderX = leftShoulder.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float leftShoulderXAbsolut = leftShoulder.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();

            float rightShoulderY = rightShoulder.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightShoulderX = rightShoulder.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float rightShoulderXAbsolut = rightShoulder.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();

            float leftElbowY = leftElbow.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float leftElbowX = leftElbow.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float leftElbowXAbsolut = leftElbow.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();

            float rightElbowY = rightElbow.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightElbowX = rightElbow.getJointMovimentByAxis(Axis.AXIS_X).getLastElementPosition();
            float rightElbowXAbsolut = rightElbow.getJointMovimentByAxis(Axis.AXIS_X).getAbsolutValue();
            if (isJoinZDistantFromJointBase(JointType.HandLeft, JointType.ShoulderLeft, 0.4f, selectedFunction) &&
                isJoinZDistantFromJointBase(JointType.HandRight, JointType.ShoulderRight, 0.4f, selectedFunction))
            {
                //Console.Beep();
                if (areSimilar(leftShoulderY, leftElbowY) && areSimilar(rightShoulderY, rightElbowY))
                {
                    if (leftElbowX== leftHandX && rightElbowX==rightHandX)
                    {
                        setPose(Pose.ZOOMBIE);
                        return true;
                    }
                }
            }
            return false;
        }
        
        public bool areSimilar(float a, float b) {
        float res;
            if(a>b){
                res = a-b;
            }else{
                res = b-a;
            }
            res = (float)Math.Round(res, 2);
            if(res<=(float)0.1){
                return true;
            }else{
                return false;
            }
        }

        
        internal bool areTwoHandsUp()
        {
            MyJoint leftHand = getMyJointByJointType(JointType.HandLeft);
            MyJoint rightHand = getMyJointByJointType(JointType.HandRight);
            MyJoint leftShoulder = getMyJointByJointType(JointType.ShoulderLeft);
            MyJoint rightShoulder = getMyJointByJointType(JointType.ShoulderRight);

            float leftHandY = leftHand.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightHandY = rightHand.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();

            float leftShoulderY = leftShoulder.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            float rightShoulderY = rightShoulder.getJointMovimentByAxis(Axis.AXIS_Y).getLastElementPosition();
            if(leftHandY>leftShoulderY &&rightHandY>rightShoulderY){
                return true;
            }
            return false;

        }

        internal bool areTwoHandsGetClose()
        {
            if (getJointMovimentStatus(JointType.HandRight, Axis.AXIS_X, 1).Equals(MovimentStatus.DECREASING) &&
               getJointMovimentStatus(JointType.HandLeft, Axis.AXIS_X, 1).Equals(MovimentStatus.INCREASING))
            {
                return true;
            }
            else {
                return false;
            }
        }

        internal bool areTwoHandGetAway()
        {
            if((getJointMovimentStatus(JointType.HandRight, Axis.AXIS_X,1).Equals(MovimentStatus.INCREASING) &&
                    getJointMovimentStatus(JointType.HandLeft, Axis.AXIS_X,1).Equals(MovimentStatus.DECREASING))){
                return true;
            }else{
                return false;
            }
        }
    }

   
    

}
